%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Algoritmo de PAN-Tompkins                     %
% Materia: Se�ales e imagenes en biomedicina    %
% Alumnos: Besada- Mantaut                      %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [Sen_Salida, Ubicacion_QRSs] = tompkins9(x,Fs,Kwindow,long_der,GRAFICOS)
RETRASO1 = 1;
RETRASO2 = 1;
RETRASO = 1;
TIEMPO_APRENDIZAJE_INICIAL = 1.5;
TIEMPO_APRENDIZAJE = 2;
ORIGEN = 50;
Primer_Pico = 1;
PER_REFRACTARIO_2 = 0.36;
PER_REFRACTARIO_1 = 0.2;
factor_FFT = 100;

Nwindow =ceil(Kwindow*Fs);
MWindow=ones(1,Nwindow);

CALCULAR_ESPECTROS = 0;

close all;

RR = zeros(1,length(x));
Indice_Picos = 1;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%          Filtrado
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

tAxis = 0:1/Fs:(length(x)-1)/Fs;    

RETRASO_Filtro_Pasabajos = 6;
RETRASO_Filtro_Pasaaltos = 16;
RETRASO_Integracion = round(Nwindow/2);
RETRASO_Total = RETRASO_Filtro_Pasabajos + RETRASO_Filtro_Pasaaltos + RETRASO_Integracion;
    
A_pasabajos = [1 -2 1];  
B_pasabajos = [1 0 0 0 0 0 -2 0 0 0 0 0 1]; 

A_pasaaltos = [1 -1];
B_pasaaltos = [-1/32 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 -1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1/32]; 


y1=filter(B_pasabajos,A_pasabajos,x);
y2=filter(B_pasaaltos,A_pasaaltos,y1);



if CALCULAR_ESPECTROS == 1
    N = length(x)*factor_FFT;
    espectro_x = fft(x,N); 
    fAxis= ([-N/2: (N-1)/2]/N) *Fs;  
    espectro_y1 = fft(y1,length(y1)*factor_FFT); 
    espectro_y2 = fft(y2,length(y2)*factor_FFT); 
end
    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

y3 = ones(1,length(y2));
y4 = ones(1,length(y2));
y5 = ones(1,length(y2));

%TODO ajustar a frecuencias variables, que la ventana de integracion
%usar el ancho en tiempo que encontramos para la ventana de integracion 
% y que sea independiente de las muestras

for i=3:length(y2)-2
    y3(i)=(-y2(i-2)-2*y2(i-1)+2*y2(i+1)+y2(i+2))/8; %DERIVADA%
    y4(i)=y3(i)^2; % CUADRADO %
    if(i<=Nwindow) % INTEGRACION %
        acum=0;
        for j=1:i
            acum = acum + y4(j);
        end
        y5(i)=acum;
    else
        acum = acum + y4(i)-y4(i-Nwindow);
        y5(i)=acum;
    end  

end

senial = y5(RETRASO_Total:length(y5));  % SE�AL PROCESADA %
senial = senial /max(senial);  % SE�AL PROCESADA NORMALIZADA %

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%% BLOQUE DE DECISI�N %%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%% APRENDIZAJE %%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%% 1ER ETAPA %%%%%%%%%%%%%%%%%%%%%%%%
% (Inicializa variables y calcula un valor medio) %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

aux = (-1)*ones(1,10);
i = long_der;

muest_aprendizaje_inicial = round(TIEMPO_APRENDIZAJE_INICIAL*Fs);
muest_aprendizaje = round(TIEMPO_APRENDIZAJE*Fs);
Indice_Picos = 0; 
suma = 0;
indice_RR_anterior = 0;
aux = zeros(1,2*long_der+1);
Candid_Searchback = 0;
ind_Candid_Searchback = 0;

valor_medio = mean(senial(ORIGEN: max(ORIGEN,muest_aprendizaje_inicial))); %CALCULA EL VALOR MEDIO DE LA ETAPA%
indice_RR_anterior = muest_aprendizaje_inicial;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%% APRENDIZAJE %%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%% 2DA ETAPA %%%%%%%%%%%%%%%%%%%%%%%%
%%%% (Establece umbrales e intervalos RR iniciales) %%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%detect = zeros(1,length(senial));
detect = ones(1,length(senial))*(-1);


for i = muest_aprendizaje_inicial + long_der + 1 : muest_aprendizaje_inicial + muest_aprendizaje - long_der
    
    Es_Pico = 1;
    for j = 1:1:2*long_der + 1
        aux(j) = senial (i) - senial(i-long_der + j - 1);
        if aux(j)< 0 
            Es_Pico = 0;
            break;
        end
    end
 
    if(Es_Pico == 1 && (i - indice_RR_anterior)/Fs > PER_REFRACTARIO_2 && senial (i) > valor_medio) % Establece QRS's iniciales %
            detect(i) =   senial(i);
            Indice_Picos = Indice_Picos + 1;            
            suma = suma + senial(i);
            RR(Indice_Picos) = (i - indice_RR_anterior)/Fs;
            Ubicacion_Picos(Indice_Picos) = i;
            indice_RR_anterior = i;    
    end
end

if Indice_Picos == 0
    RR(1) = 0.85;
    Indice_Picos = 1;
    Ubicacion_Picos(1) = muest_aprendizaje_inicial + muest_aprendizaje;
end

spki = suma/Indice_Picos;    %ESTIMADOR DE PICO DE SE�AL INICIAL%
npki = 0.1*spki;         %ESTIMADOR DE PICO DE RUIDO INICIAL%   

RR_Average1 = 0;
for i = 1:1:Indice_Picos
    RR_Average1 = RR_Average1 + RR(i);
end
RR_Average1 = RR_Average1/Indice_Picos;

RR_MISSED_LIMIT = 1.66*RR_Average1;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%% DETECCION %%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Pico_Ruido_Max = 0;

npk = zeros(1,length(senial));
spk = zeros(1,length(senial));

i = muest_aprendizaje_inicial + muest_aprendizaje + long_der + 1;
while i <= length(senial)-long_der
    
    Thres1 = npki + 0.25*(spki-npki);   % ACTUALIZA UMBRALES %
    Thres2=0.5*Thres1;                  % ACTUALIZA UMBRALES %
    npk(i) =  npki;
    spk(i) =  spki;
    
    Es_Pico = 1;
    for j = 1:1:2*long_der + 1
        aux(j) = senial (i) - senial(i-long_der + j - 1);
        if aux(j)< 0 
            Es_Pico = 0;
            break;
        end
    end
    
    if(Es_Pico == 1) % AQU� EMPIEZA EL �RBOL DE DECISI�N %
                                                        
        Peaki = senial(i);
        RR_Potencial = (i - Ubicacion_Picos(Indice_Picos))/Fs; 
        
        if( Peaki>Thres2 && RR_Potencial > PER_REFRACTARIO_1)  
                if Peaki > Thres1  
                    if RR_Potencial > PER_REFRACTARIO_2
                        detect(i) =   Peaki;
                        Indice_Picos = Indice_Picos + 1;
                        Ubicacion_Picos(Indice_Picos) = i;
                        RR(Indice_Picos) = RR_Potencial ;
                        
                        if Indice_Picos > 8 RR_Average1 = RR_Average1 + (RR(Indice_Picos) - RR(Indice_Picos - 8))/8;
                        else RR_Average1 = (RR_Average1 * (Indice_Picos - 1) + RR(Indice_Picos))/(Indice_Picos);
                        end
                        
                        RR_MISSED_LIMIT = 1.66*RR_Average1;
                        Pico_Ruido_Max = max(Candid_Searchback,Pico_Ruido_Max);
                        spki = 0.125*Peaki +0.875*spki;
                        npki=0.125*Pico_Ruido_Max + 0.875*npki;
                        Candid_Searchback = 0;
                        ind_Candid_Searchback = 1;
                        Pico_Ruido_Max = 0;
                    
                    else
                        if Es_Onda_T(i) == 0
                            detect(i) =   Peaki;
                            Indice_Picos = Indice_Picos + 1;
                            Ubicacion_Picos(Indice_Picos) = i;
                            RR(Indice_Picos) = RR_Potencial ;
                            
                            if Indice_Picos > 8 RR_Average1 = RR_Average1 + (RR(Indice_Picos) - RR(Indice_Picos - 8))/8;
                            else RR_Average1 = (RR_Average1 * (Indice_Picos - 1) + RR(Indice_Picos))/(Indice_Picos);
                            end
                            
                            RR_MISSED_LIMIT = 1.66*RR_Average1;
                            Pico_Ruido_Max = max(Candid_Searchback,Pico_Ruido_Max);
                            spki = 0.125*Peaki +0.875*spki;
                            npki=0.125*Pico_Ruido_Max + 0.875*npki;
                            Candid_Searchback = 0;
                            ind_Candid_Searchback = 1;
                            Pico_Ruido_Max = 0;
                        else
                            if Peaki > Pico_Ruido_Max Pico_Ruido_Max = Peaki; end;  
                        end
                    end
                else
                    if RR_Potencial > PER_REFRACTARIO_2  | Es_Onda_T(i) == 0
                        if (Peaki > Candid_Searchback)   
                            Pico_Ruido_Max = max(Candid_Searchback,Pico_Ruido_Max);                  
                            Candid_Searchback = Peaki;
                            ind_Candid_Searchback = i;
                        else
                            Pico_Ruido_Max = max(Peaki,Pico_Ruido_Max);    
                        end
                    else
                        if Peaki > Pico_Ruido_Max Pico_Ruido_Max = Peaki; end;  
                    end
                end
        else
            if Peaki > Pico_Ruido_Max Pico_Ruido_Max = Peaki; end; 
        end

       end

   if (i - Ubicacion_Picos(Indice_Picos))/Fs > RR_MISSED_LIMIT
                
        if (Candid_Searchback ~= 0)
            detect(ind_Candid_Searchback) =   Candid_Searchback;
            Indice_Picos = Indice_Picos + 1;
            Ubicacion_Picos(Indice_Picos) = ind_Candid_Searchback;
            RR(Indice_Picos) = (Ubicacion_Picos(Indice_Picos) - Ubicacion_Picos(Indice_Picos - 1))*(1/Fs);
            
            if Indice_Picos > 8 RR_Average1 = RR_Average1 + (RR(Indice_Picos) - RR(Indice_Picos - 8))/8;
            else RR_Average1 = (RR_Average1 * (Indice_Picos - 1) + RR(Indice_Picos))/(Indice_Picos);
            end
            
            RR_MISSED_LIMIT = 1.66*RR_Average1;
            spki = 0.25*Candid_Searchback + 0.75*spki;
            npki=0.25*Pico_Ruido_Max + 0.75*npki;
            i = ind_Candid_Searchback;
            Candid_Searchback = 0;    
            ind_Candid_Searchback = 1;
            Pico_Ruido_Max = 0;
        
        end
        
   end   % AQU� TERMINA EL �RBOL DE DECISI�N %
i = i+1;    
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%% GENERACI�N DE LA SALIDA %%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Sen_Salida = detect;
Ubicacion_QRSs = Ubicacion_Picos;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%% GR�FICOS %%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if GRAFICOS == 1
    
    figure(1);
    subplot(3,1,1);
    plot(x(1:length(x)));
    title('Se�al original (x)')

    subplot(3,1,2);
    plot(y1(1:length(y1)));
    title('Se�al original filtrada pasabajos (y1)')

    subplot(3,1,3);
    plot(y2(1:length(y2)));
    title('Se�al original filtrada pasabanda (y2)')

    figure(2);

    subplot(3,1,1);
    plot(y3(1:length(y3)));
    title('Derivada (y3)')

    subplot(3,1,2);
    plot(y4(1:length(y4)));
    title('Cuadrado (y4)')

    subplot(3,1,3);
    plot(senial(1:length(senial)));
    title('Integraci�n (senial)')

    if CALCULAR_ESPECTROS == 1
        figure(6)
    
        subplot(3,1,1);
        plot(fAxis, fftshift(abs(espectro_x)));
        title('Espectro de la se�al original')
    
        subplot(3,1,2);
        plot(fAxis, fftshift(abs(espectro_y1)));
        title('Espectro de la se�al filtrada Pasabajos')
    
        subplot(3,1,3);
        plot(fAxis, fftshift(abs(espectro_y2)));
        title('Espectro de la se�al filtrada Pasabanda')
    end
    
    figure(7)
    plot(Sen_Salida(1:length(Sen_Salida)),'.r');
    title('Se�al de detecci�n (Sen_Salida) y se�al original')
    hold
    plot(senial(1:length(senial)));
    sen_aux = (x - mean(x))/max(x);
    plot(sen_aux(1:length(sen_aux)),'g');
    
    plot(npk,'k-');
    plot(spk,'c-');
    
    
    fprintf('La longitud de la se�al original es: %i\n', length(x));
    fprintf('La longitud de la se�al filtrada LP es: %i\n', length(y1));
    fprintf('La longitud de la se�al filtrada BP es: %i\n', length(y2));
    fprintf('La longitud de la se�al derivada es: %i\n', length(y3));
    fprintf('La longitud de la se�al al cuadrado es: %i\n', length(y4));
    fprintf('La longitud de la se�al integrada es: %i\n', length(y5));
    fprintf('La longitud de la se�al "senial" es: %i\n', length(senial));
end
end

function [Onda_T] = Es_Onda_T(i)
        Onda_T = 0;    
end

%function  [Sen_Salida, Ubicacion_QRSs] = tompkinsFinal(x,Fs)

%[Sen_Salida, Ubicacion_QRSs] =  tompkins9(x,Fs,0.13,10,0);  

%end



